#include <tasks_manager/task_gen_interface.h>




PathGenInterface::PathGenInterface(){
    // 兼容服务
    __all_sparse_client = __nh.serviceClient<cln_msgs::CoverPathGen>("/all_sparse_path");
    __teach_path_gen_client = __nh.serviceClient<cln_msgs::GenerateTeachPath>("/teach_path_generate");
    __hand_poly_gen_client = __nh.serviceClient<cln_msgs::GenerateHandPoly>("/hand_poly_gen");
    __hand_poly_preview_client = __nh.serviceClient<cln_msgs::GenerateHandPoly>("/preview_hand_poly");
    // 新版本接口
    __hand_path_client = __nh.serviceClient<v3_msgs::PathGenerateManager>("/hand_path_server");
    __p2p_path_client = __nh.serviceClient<v3_msgs::PathGenerateManager>("/p2p_path_server");
    __cover_path_client = __nh.serviceClient<v3_msgs::PathGenerateManager>("/cover_path_server");
}


PathGenInterface::~PathGenInterface(){

}


/*******************************************
 * 旧接口 ROS服务接口
 *******************************************/

// 生成区域任务路径
Status PathGenInterface::GenPolygonTaskWithClnMsg(TaskSingle& task_packed, bool only_gen_area){
    Status status;
    // 请求路径生成
    cln_msgs::CoverPathGen client_data;

    try{
        // 是否为手推区域
        TaskBase task_base = task_packed.task_base;
        if(task_base.path_gen.path_types.empty()){
            throw(string("path_types is empty"));
        }
        client_data.request.plan_area_type = task_base.path_gen.path_types.front() == PathGenerateType::MANUAL_METHOD 
            ? client_data.request.HANDPUSH : client_data.request.CUSTOMIZATION;
        client_data.request.offset = task_base.path_gen.offset;
        client_data.request.raw = __msg_convert.TaskBase2CleanTask(task_base);
        client_data.request.raw.state = client_data.request.raw.FABRICATING;
        client_data.request.only_gen_area = only_gen_area;
        cout << "[GenPolygonTaskWithClnMsg] map_name: " << client_data.request.raw.map_name 
            << ", task_name: " << client_data.request.raw.task_name 
            << ", type: " << int(client_data.request.raw.type) 
            << ", clean_mode: " << int(client_data.request.raw.clean_mode) 
            << ", state: " << int(client_data.request.raw.state) 
            << ", global_area size: " << client_data.request.raw.global_area.points.size() 
            << ", danger_areas size: " << client_data.request.raw.danger_areas.size() 
            << endl;

        __all_sparse_client.call(client_data);
        // 解析返回数据
        if(!client_data.response.success){
            throw(client_data.response.message);
        }
        // 封装路径信息
        cout << "[GenPolygonTaskWithClnMsg] task_packed.subtasks.size = " << client_data.response.packed.subtasks.size() << endl;
        TaskSingle task_single = __msg_convert.CleanTask2TaskSingle(client_data.response.packed);
        cout << "[GenPolygonTaskWithClnMsg] task_single.task_base.task_area: " << task_single.task_base.task_area << endl;
        task_packed.task_base.task_area = task_single.task_base.task_area;
        task_packed.task_base.paths = task_single.task_base.paths;
        task_packed.task_base.edge_paths = task_single.task_base.edge_paths;
    }catch(string& error){
        status.success = false;
        status.message = error;
    }
    return status;
}

// 生成手推路线
Status PathGenInterface::GenHandLineWithClnMsg(TaskSingle& task_packed){
    Status status;
    // 请求路径生成
    cln_msgs::GenerateTeachPath client_data;
    TaskBase task_base = task_packed.task_base;
    switch(task_base.path_gen.path_action){
        case PathGenerateAction::START_RECORD:
            client_data.request.flag = client_data.request.START_RECOED;
            break;
        case PathGenerateAction::CANCEL_RECORD:
            client_data.request.flag = client_data.request.CANCEL_RECOED;
            break;
        case PathGenerateAction::FINISHED_RECORD:
            client_data.request.flag = client_data.request.SAVE_RECOED;
            client_data.request.path_name = task_base.task_name;
            client_data.request.clean_mode_name = task_base.utils_mode;
            // speed_level
            if(task_base.task_speed == TaskSpeed::LOW_SPEED)
                client_data.request.speed_level = 1;
            if(task_base.task_speed == TaskSpeed::MIDDLE_SPEED)
                client_data.request.speed_level = 2;
            if(task_base.task_speed == TaskSpeed::HIGH_SPEED)
                client_data.request.speed_level = 3;
            // sweep_mode
            if(!task_base.path_gen.path_types.empty()){
                if(task_base.path_gen.path_types.front() == PathGenerateType::MANUAL_METHOD)
                    client_data.request.sweep_mode = client_data.request.NORMAL_CLEAN;
                if(task_base.path_gen.path_types.front() == PathGenerateType::EDGE_METHOD)
                    client_data.request.sweep_mode = client_data.request.PERIMETER_CLEAN;
            }
            break;
        default:
            break;
    }
    try{
        __teach_path_gen_client.call(client_data);
        // 解析返回数据
        if(client_data.response.success != 0){
            throw(client_data.response.message);
        }
        TaskPath task_path;
        task_path.poses = client_data.response.poses;
        task_packed.task_base.edge_paths.emplace_back(task_path);

    }catch(string& error){
        status.success = false;
        status.message = error;
    }
    return status;
}



// 生成手推区域轮廓
Status PathGenInterface::GenHandPolygonWithClnMsg(TaskSingle& task_packed, bool is_preview){
    Status status;
    // 请求路径生成
    cln_msgs::GenerateHandPoly client_data;
    TaskBase task_base = task_packed.task_base;
    switch(task_base.path_gen.path_action){
        case PathGenerateAction::START_RECORD:
            client_data.request.flag = client_data.request.START_RECOED;
            break;
        case PathGenerateAction::CANCEL_RECORD:
            client_data.request.flag = client_data.request.CANCEL_RECOED;
            break;
        case PathGenerateAction::FINISHED_RECORD:
            client_data.request.flag = client_data.request.SAVE_RECOED;
            break;
        default:
            break;
    }
    try{
        if(is_preview){
            __hand_poly_preview_client.call(client_data);
        }  
        else{
            __hand_poly_gen_client.call(client_data);
        }
        if(client_data.response.success != 0){
            throw(client_data.response.message);
        }
        cout << "[GenHandPolygonWithClnMsg] polygen size: " << client_data.response.poly.points.size() << endl;
        // 解析返回数据
        for(auto& point : client_data.response.poly.points){
            geometry_msgs::Pose pose;
            pose.position.x = point.x;
            pose.position.y = point.y;
            pose.position.z = point.z;
            task_packed.task_base.polygon.poses.emplace_back(pose);
        }
    }catch(string& error){
        status.success = false;
        status.message = error;
    }
    return status;
}


/*******************************************
 * 新接口 ROS服务接口
 *******************************************/
Status PathGenInterface::PathGenerateRequest(ros::ServiceClient& client, TaskSingle& task_packed, bool is_preview){
    Status status;
    v3_msgs::PathGenerateManager client_data;
    try{
        client_data.request.control_mode = __msg_convert.TaskControlMode2V3Msgs(task_packed.task_base.control_mode);
        client_data.request.task_speed = __msg_convert.TaskSpeed2V3Msgs(task_packed.task_base.task_speed);
        client_data.request.path_gen = __msg_convert.PathGenerate2V3Msgs(task_packed.task_base.path_gen);
        // 起点和多边形轮廓赋值
        client_data.request.path_gen.start = task_packed.task_base.start;
        client_data.request.path_gen.polygon = task_packed.task_base.polygon;
        client_data.request.is_preview = is_preview;
        cout << "[PathGenerateRequest] polygon size: " << client_data.request.path_gen.polygon.poses.size() 
            << ", danger_areas size: " << client_data.request.path_gen.danger_areas.size() 
            << endl;

        if(!client.call(client_data)){
            throw(string(client.getService() + " call failed!"));
        }
        if(!client_data.response.response.success){
            throw(client_data.response.response.message);
        }  
        // 更新数据信息 
        cout << "[GenHandPathWithV3Msg] paths size: " << client_data.response.paths.size() << endl;
        for(auto& task_path : client_data.response.paths){
            cout << "[GenHandPathWithV3Msg] poses size: " << task_path.poses.size() << endl;
            task_packed.task_base.paths.emplace_back(__msg_convert.V3Msgs2TaskPath(task_path));
        }
        for(auto& task_path : client_data.response.edge_paths){
            task_packed.task_base.edge_paths.emplace_back(__msg_convert.V3Msgs2TaskPath(task_path));
        }   
        task_packed.task_base.estimated_time = client_data.response.estimated_time;
        task_packed.task_base.task_area = client_data.response.task_area;
    }catch(string& error){
        status.success = false;
        status.message = error;
    }    
    return status;
}

// 生成区域任务路径
Status PathGenInterface::GenPolygonTaskWithV3Msg(TaskSingle& task_packed, bool is_preview){
    Status status;
    try{
        status = PathGenerateRequest(__cover_path_client, task_packed, is_preview);
    }catch(string& error){
        status.success = false;
        status.message = error;
    }    
    return status;
}

// 生成手推路线
Status PathGenInterface::GenHandPathWithV3Msg(TaskSingle& task_packed, bool is_preview){
    Status status;
    try{
        status = PathGenerateRequest(__hand_path_client, task_packed, is_preview);
    }catch(string& error){
        status.success = false;
        status.message = error;
    }    
    return status;
}

// 生成P2P轮廓
Status PathGenInterface::GenP2PPathWithV3Msg(TaskSingle& task_packed){
    Status status;
    try{
        status = PathGenerateRequest(__p2p_path_client, task_packed, false);
    }catch(string& error){
        status.success = false;
        status.message = error;
    }    
    return status;
}